from naoqi import ALProxy
from walkToObstacle import walkToObstacle
from walkToObstacle import walkToObstacleTOWARDversion
from walkToObstacle import alignToObstacle
from walkToObstacle import walkToEnd
from obstacle import obstacle
from obstacle import obstacle2
from obstacle import obstacleHENGZHEZOU
from obstacle import obstacleHENGZHEZOU2


def launch(robotIP, PORT=9559):
    motionProxy = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
    valueProxy = ALProxy("ALMemory", robotIP, PORT)
    motionProxy.wakeUp()
    postureProxy.goToPosture("StandInit", 0.5)
    angleINIT = valueProxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value")

    motionProxy.moveTo(0.01, 0, 0)
    walkToObstacleTOWARDversion(robotIP, PORT, angleINIT, "blue")
    obstacleHENGZHEZOU2(robotIP, PORT, angleINIT, "blue")

    alignToObstacle(robotIP, PORT, "red", angleINIT)
    walkToObstacleTOWARDversion(robotIP, PORT, angleINIT, "red")
    obstacleHENGZHEZOU(robotIP, PORT, angleINIT, "red")

    alignToObstacle(robotIP, PORT, "yellow", angleINIT)
    walkToObstacleTOWARDversion(robotIP, PORT, angleINIT, "yellow")
    obstacleHENGZHEZOU2(robotIP, PORT, angleINIT, "yellow")
    alignToObstacle(robotIP, PORT, "blue", angleINIT)

    walkToEnd(robotIP, PORT, angleINIT)

    motionProxy.rest()


if __name__ == '__main__':
    robotIP = "169.254.219.86"
    PORT = 9559
    valueProxy = ALProxy("ALMemory", robotIP, PORT)
    motionProxy = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
    motionProxy.wakeUp()
    postureProxy.goToPosture("StandInit", 0.5)
    launch(robotIP, PORT)
